#!/usr/bin/env python
# -*- coding: utf-8 -*-
# @Author: obsidian
# @Date:   2016-01-25 15:20:00
# @Last Modified by:   copbint
# @Last Modified time: 2016-03-03 13:03:04
import numpy as np
import matplotlib.pyplot as plt
x = []
y = []
theta = []
step = 5
# p = \
# 	[
# 		[[0,0],0,0],#0
# 		[[0,800.0],1,1],#1
# 		[[-61.9861,1270.8309],0,0],#2
# 		[[-247.8,1964.36],1,1],#3
# 		[[-429,2403],0,0],
# 		[[-780.46,3010.9],1,1],#5
# 		[[-1068.6490,3386.0864],0,0],
# 		[[-1467.51,3784.789],1,-1],
# 		[[-1756.4161,4161.5],0,0],
# 		[[-2263.44,5040.76],1,-1],
# 		[[-2433.1198,5435.6007],0,0],#10
# 		[[-2624.9571,6087.2938],1,-1],
# 		[[-2241.2391,6600.2564],0,0],
# 		[[-1729.4402,6600.2564],1,1],
# 		[[-1356.8765,6740.5863],1,-1],
# 		[[-984.3007,6880.9161],0,0],
# 		[[126.5264,6880.91],1,1],
# 		[[419.7560,7008.8565],0,0],
# 		[[1657.5246,8342.9408],1,1],
# 		[[1651.2126,9254.2490],0,0],
# 		[[1003.1025,9933.6680],1,-1],
# 		[[1049.4999,10791.8316],0,0],
# 		[[1512.8070,11182.8239],1,1],
# 		[[1811.6046,11830.4200],0,0],
# 		[[1811.6046,12830.4200],0,0],
# 	]
p = \
	[
		[[0,0],0,0],#0
		[[0,752.4364],1,1],#1
		[[-44.2964,1088.9012],0,0],#2
		[[-323.4633,2130.7662],1,1],#3
		[[-453.3339,2444.3014],0,0],
		[[-779.46,3009.3],1,1],#5
		[[-1068.6490,3386.0864],0,0],
		[[-1467.51,3784.789],1,-1],
		[[-1756.4161,4161.5],0,0],
		[[-2276.4571,5062.4352],1,-1],
		[[-2426.3078,5424.2066],0,0],#10
		[[-2605.5095,6096.7288],1,-1],#11
		[[-2220.1392,6600.2564],0,0],#12
		[[-1664.5702,6600.2564],1,1],#13
		[[-1381.7275,6717.4137],0,0],#14
		[[-1333.2480,6765.8935],1,-1],#15
		[[-1035.9083,6882.7877],0,0],#16
		[[31.6170,6844.0727],1,1],
		[[334.4848,6966.6055],0,0],
		[[1657.5246,8342.9408],1,1],
		[[1651.2126,9254.2490],0,0],
		[[1003.1025,9933.6680],1,-1],#21
		[[1049.4999,10791.8316],0,0],
		[[1512.8070,11182.8239],1,1],
		[[1811.6046,11830.4200],0,0],
		[[1811.6046,12830.4200],0,0],
	]
C = \
	[
		[],
		[-1300.0000,752.4363],
		[],
		[-1579.1669,1794.3014],
		[],
		[-2354.9829,2099.7525],#5
		[],
		[-180,5070.1230],
		[],
		[-997.4190,5812.4352],
		[],#10
		[-2220.1392,6200.2564],
		[],
		[-1664.5702,7000.2564],
		[],
		[-1050.4053,6483.0505],
		[],
		[46.1140,7243.8099],
		[],
		[1170.0299,8795.2402],
		[],
		[1430.0299,10340.9288],
		[],
		[970.0299,11825.9885],
		[],
		[]
	]


def line(start,stop):
	global x,y
	if stop[0] - start[0] == 0:
		if stop[1] - start[1] > 0:
			t_l = np.pi/2
		elif stop[1] - start[1] < 0:
			t_l = - np.pi / 2
	else:
		t_l = np.arctan((stop[1] - start[1]) / (stop[0] - start[0]))
	if stop[0] - start[0] < 0:
		t_l = t_l + np.pi
	x_step = step * np.cos(t_l)
	y_step = step * np.sin(t_l)
	l = np.sqrt((start[0] - stop[0]) ** 2 + (start[1] - stop[1]) ** 2)
	for i in range(int(abs(l / step))):
		x.append(i * x_step + start[0])
		y.append(i * y_step + start[1])
		tmp = np.pi / 2 - t_l
		while tmp > np.pi:
			tmp -= 2 * np.pi
		while tmp < - np.pi:
			tmp += 2 * np.pi
		theta.append(tmp)
def circle(start,stop,center,direc):
	global x,y,theta
	r = np.sqrt((center[1] - start[1]) * (center[1] - start[1]) + 
				(center[0] - start[0]) * (center[0] - start[0]))
	theta_step = step / r

	if center[0] - start[0] == 0:
		if start[1] - center[1] > 0:
			theta_start = np.pi/2
		elif start[1] - center[1] < 0:
			theta_start = - np.pi / 2
	else:
		theta_start = np.arctan((center[1] - start[1]) / (center[0] - start[0]))
		if start[0] - center[0] < 0:
			theta_start = theta_start + np.pi
	if center[0] - stop[0] == 0:
		if stop[1] - center[1] > 0:
			theta_stop = np.pi / 2
		elif stop[1] - center[1] < 0:
			theta_stop = - np.pi / 2
	else:
		theta_stop = np.arctan((center[1] - stop[1]) / (center[0] - stop[0]))
		if stop[0] - center[0] < 0:
			theta_stop = theta_stop + np.pi
	if theta_start < 0:
		theta_start += 2 * np.pi
	if theta_stop < 0:
		theta_stop += 2 * np.pi
	if direc == -1:
		theta_step = -theta_step
		delta = theta_stop - theta_start
		while delta < 0:
			delta += 2 * np.pi
		delta =2 * np.pi - delta
	if direc == 1:
		theta_step = theta_step
		delta = theta_stop - theta_start
		while delta < 0:
			delta += 2 * np.pi
	#print delta 
	for i in range(int(abs(delta / theta_step)) + 1):
		x.append(center[0] + r * np.cos(theta_start + i * theta_step))
		y.append(center[1] + r * np.sin(theta_start + i * theta_step))
	
		if direc == 1 :
			tmp = - theta_start - i * theta_step
		if direc == -1 :
			tmp = np.pi - theta_start - i * theta_step
		while tmp > np.pi:
			tmp -= 2 * np.pi
		while tmp < - np.pi:
			tmp += 2 * np.pi
		theta.append(tmp)
left = [];
right = [];
for i in range(len(p) - 1):
	if p[i][1] == 0:
		line(p[i][0],p[i+1][0])
	else:
		circle(p[i][0],p[i+1][0],C[i],p[i][2])
for i in range(len(x)):
	right.append([[round(-x[i],4),round(y[i],4)],round(-theta[i],4)])
for i in range(len(x)):
	left.append([[round(x[i],4),round(y[i],4)],round(theta[i],4)])

f = open('Route_right14.c', 'w')
f.write('#include \"pointpath.h\" \n const struct Path_point path[PATH_LEN] = \n')
f.write(str(right).replace('[','{').replace(']','}').replace('}, {','},\n {') + ';\n')
f.close()
f = open('Route_left14.c', 'w')
f.write('#include \"pointpath.h\" \n const struct Path_point path[PATH_LEN] = \n')
f.write(str(left).replace('[','{').replace(']','}').replace('}, {','},\n {') + ';\n')
f.close()

#plt.plot(x,y,'.')
#print y
plt.plot(x,y,'o')
plt.show()
